#pragma once

#include <fstream>
#include <iostream>
#include <cmath>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include "../include/common.hpp"
#include <array> // array
#include <cstddef> // size_t
#include <cstdint> // int
#include <string> // string


using namespace std;
using namespace cv;
class track
{
private:
    struct jump_point_pair
    {
        int left;
        int right;
    };
    //跳变点对数组
    jump_point_pair jump_point_pairs[COLSIMAGE];
    POINT pointSpurroad;
public:
    int effictive_line_num;
    int effective_l;
    int effective_r;
    //有效行
    bool effective_row[COLSIMAGE];

    //原图像
    Mat img;
    //灰度图在imgprocess那里 用完释放省空间
    //二值化Mat
    Mat binary_mat;
    Mat base_map;
    //二值化数组 
    int index[ROWSIMAGE][COLSIMAGE];

    track(){};
    //slope斜率作为点有效标志
    POINT l_border[ROWSIMAGE];
    POINT r_border[ROWSIMAGE];
    POINT mid_line_list[ROWSIMAGE];

    vector<POINT> spurroad;           // 保存岔路信息

    void find_mid_line()
    {
        effective_l=0;
        effective_r=0;
        effictive_line_num=0;
        //有效行清0
        effective_row[COLSIMAGE]={false};
        
        bool if_spurroad=false;
        //从下到上遍历的过程中必须是
        //清空vector
        spurroad.clear();
        //首行不做处理
        int i,j,k;
        //每一张图像都先置成flase
        //张
        // int counterSpurroad = 0;                       // 岔路识别标志


        //跳变点对计数
        int pair_num;

        //距离中线绝对值
        //行
        int length_r;
        int length_l;
        for(i=ROWSIMAGE-1;i>=0;i--)
        {
            l_border[i].x=i;
            r_border[i].x=i;
            l_border[i].y=0;
            r_border[i].y=COLSIMAGE-1;
        }
        //从下到上
        for(i=ROWSIMAGE-1;i>=0;i--)
        {

            //距离中线绝对值
            length_r=COLSIMAGE;
            length_l=COLSIMAGE;
            pair_num=0;
            for(j=0;j<COLSIMAGE;j++)
            {
    //找跳变点对
                //只有找到左跳变点的时候才会++；不会有单个右跳变点的 都是成对的
                //处理0与319上
                //注意边线上点的处理防止数组越界
                if(j==0)
                {
                    if(index[i][j]==255)
                    {jump_point_pairs[pair_num].left=j;}
                }
                else if(j==COLSIMAGE-1)
                {
                    if(index[i][j]==255)
                    {
                        jump_point_pairs[pair_num].right=j;
                        pair_num++;
                    }
                }
                //左跳变点 黑入白
                //边线标在黑色点上
                else
                {
                    if(index[i][j]==0&&index[i][j+1]==255)
                    {
                        jump_point_pairs[pair_num].left=j;
                    }
                    if(index[i][j-1]==255&&index[i][j]==0)
                    {
                        jump_point_pairs[pair_num].right=j;
                        pair_num++;
                    }
                }
            }
        // //对左右边线赋值；岔路
        //     r_border[i].y=COLSIMAGE;圆环全黑乱赋值很有可莪能使这玩意害的


            //之前跳变对给了二分之一屏幕宽度会上溢出现段错误，扩容之后，断点多余一定值之后直接跳出去，舍弃当前行（就是直接变成左右边线值），连岔路点一并舍弃
            if(pair_num==1){
                //判断是全白,舍弃
                if((jump_point_pairs[0].left==0)&&(jump_point_pairs[0].right==COLSIMAGE-1))
                {
                    if_spurroad=true; 
                }
                else
                {
                    l_border[i].y=jump_point_pairs[0].left;
                    r_border[i].y=jump_point_pairs[0].right;  
                    effective_row[i]=true;
                    if_spurroad=true;
                }
            }
            if(pair_num==2){
                //因为左右路都被截断了，所以直接判断色块的宽度
                if((jump_point_pairs[0].right-jump_point_pairs[0].left)>(jump_point_pairs[1].right-jump_point_pairs[1].left))
                {
                    l_border[i].y=jump_point_pairs[0].left;
                    r_border[i].y=jump_point_pairs[0].right;
                }else if((jump_point_pairs[0].right-jump_point_pairs[0].left)<(jump_point_pairs[1].right-jump_point_pairs[1].left))
                {
                    l_border[i].y=jump_point_pairs[1].left;
                    r_border[i].y=jump_point_pairs[1].right;    
                }
                //岔路
                if(if_spurroad==true)
                {
                    pointSpurroad.x=i;
                    pointSpurroad.y=jump_point_pairs[0].right;
                    spurroad.push_back(pointSpurroad);
                    if_spurroad=false;
                }
                effective_row[i]=true;
            }
            if(pair_num>2)
            {
                for(int ii=0;ii<pair_num;ii++)
                {
                    if(abs(jump_point_pairs[ii].left-jump_point_pairs[ii].right)>=COLSIMAGE/2)
                    {
                        l_border[i].y=jump_point_pairs[ii].left;
                        r_border[i].y=jump_point_pairs[ii].right;
                    }
                }
            }
//全黑或全小于一定值的时候给他干掉
            if(l_border[i].y!=0&&r_border[i].y!=COLSIMAGE-1)
            {
                effictive_line_num++;
            }else if(l_border[i].y!=0)
            {
                effective_l++;
            }else if(r_border[i].y!=COLSIMAGE-1){
                effective_r++;
            }

        }
    }



    void drawImage(Mat &binary_mat)
    {
        for (int i = 0; i < ROWSIMAGE; i++)
        {
            circle(binary_mat, Point(l_border[i].y,i), 1,Scalar(0, 255, 0), -1); // 绿色点
            circle(binary_mat, Point(r_border[i].y,i), 1,Scalar(0, 255, 255), -1); // 黄色点
        }
        for (int i = 0; i < spurroad.size(); i++)
        {
            circle(binary_mat, Point(spurroad[i].y, spurroad[i].x), 3,Scalar(255, 0, 0), -1); // 红色点
            // putText(binary_mat, to_string(spurroad[i].slope),  Point(spurroad[i].y, spurroad[i].x-20),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(255, 0, 0), 1, CV_AA);
        }

        // putText(binary_mat, "test", Point(COLSIMAGE - 100, ROWSIMAGE - 50),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    }


    typedef struct Point_
    {
        int x;
        int y;
    }Point_;



    typedef struct stack
    {
        int top;
        int MAX;
        Point_ *data;
    }stack;


    void InitStack(stack *s,int max,Point_ *data)
    {
        s->MAX = max;
        s->top = -1;
        s->data = data;
    }


    int FullStack(stack * s)
    {
        return ((s->top)+1)==(s->MAX);

    }

    int EmptyStack(stack *s)
    {
        return (s->top) == -1;
    }

    int PushStack(stack * s, int x, int y)
    {
        if(FullStack(s))return 0;
        Point_ a;
        a.x = x;
        a.y = y;
        s->data[s->top+1] = a;
        s->top = s->top+1;
        return 1;
    }

    Point_ PopStack(stack *s)
    {
        if(EmptyStack(s))
        {
            Point_ a;
            a.x = 0xff;a.y = 0xff;
            return a;
        }
        Point_ ret = s->data[s->top];
        s->top = s->top-1;
        return ret;
    }







    unsigned char basemap[ROWSIMAGE][COLSIMAGE];

    int Search_Mid_Start( void )
    {
        int i, j, Cnt = 0, Sum= 0;
        for( i = ROWSIMAGE - 1; i >= ROWSIMAGE - 5; i -- )
        {
            for( j = 0; j < COLSIMAGE; j ++ )
            {
                if( index[ i ][ j ] == 255 )
                {
                    Cnt ++;
                    Sum += j;
                }
            }
        }
        return Sum / Cnt;
    }


    int Search_Mid_Start_( int Start_Row )
    {
        int i, j, Cnt = 0, Sum= 0;
        for( i = Start_Row - 1; i >= Start_Row - 5; i -- )
        {
            for( j = 0; j < COLSIMAGE; j ++ )
            {
                if( index[ i ][ j ] == 255 )
                {
                    Cnt ++;
                    Sum += j;
                }
            }
        }
        return Sum / Cnt;
    }


    void searchimg(int x, int y)//閫氳繃allmap灏哹asemap涓殑鐧界偣鎻忕粯鍑烘潵锛屽苟灏哸llmap涓櫧榛戝垎鐣岀偣鐢ㄧ孩鑹?=2鎻忕粯鍑烘潵
    {
        Point_ a;
        int Top_Now = 239;
        a.x = x; a.y = y;
        stack s;
        Point_ data[77000];  //MAXSIZE閫掑綊娣卞害
        memset(basemap, 0, sizeof(basemap));
        InitStack(&s,77000,data);
        PushStack(&s,x,y);
        while(!EmptyStack(&s))//绌烘爤杩斿洖1锛屽嵆涓嶆槸绌烘爤鐨勮瘽杩涘叆while
        { 
            Point_ a;
            a = s.data[s.top];
            if(index[a.y][a.x] == 255)
            {
                if( basemap[a.y][a.x] == 0 )//濡傛灉base閲岃繖涓偣鏄粦鐐癸紝鍒欐妸浠栨敼涓虹櫧鐐?
                {
                    basemap[a.y][a.x] = 255;

                }//涓婁釜鐐逛笂涓嬪乏鍙冲摢閲屾湁榛戠偣鍝噷鍏ユ爤
                if (a.x > 0 && basemap[a.y][a.x - 1] == 0)//==1鐨勬剰鎬濇槸basemap娌℃湁琚亶鍘嗚繃
                    PushStack(&s,a.x-1,a.y);
                else if (a.x < COLSIMAGE - 1 && basemap[a.y][a.x + 1] == 0)
                    PushStack(&s,a.x+1,a.y);
                else if (a.y < ROWSIMAGE - 1 && basemap[a.y + 1][a.x] == 0)
                    PushStack(&s,a.x,a.y+1);
                else if (a.y > 0 && basemap[a.y - 1][a.x] == 0)
                    PushStack(&s,a.x,a.y-1);

                else PopStack(&s);
            }
            else
            {
                basemap[a.y][a.x] = 255;
                PopStack(&s);
            }
        }
    }



    void basemap_generate(int grow_point)
    {
        int trytry=Search_Mid_Start_(ROWSIMAGE-grow_point);//最下面一行的中心点
        if(index[(ROWSIMAGE-grow_point)][trytry]==0)
        {
            find_mid_line();
            return;
        }
        searchimg(trytry,(ROWSIMAGE-grow_point));//生成basemap
        for(int i=0;i<ROWSIMAGE;i++)//basemap返回到index
        {
            for(int j=0;j<COLSIMAGE;j++)
            {
                index[i][j]=basemap[i][j];
            }
        }
        find_mid_line();
    }
    int Right_Lost_Flag[ROWSIMAGE];
    int Left_Lost_Flag[ROWSIMAGE];

    void Get_Line_And_Lost()
    {
            int i, j, left_border, right_border;//临时存储赛道位置
            for( i = ROWSIMAGE - 1; i >= 0; i -- )
            {
                if( i == ROWSIMAGE - 1 )//第一次扫线
                {
                    int my_Mid = Search_Mid_Start_( i );
                    for ( j = my_Mid; j < COLSIMAGE - 1; j ++ )
                    {
                        if ( index[i][j] == 2 )//找到2 为index边界
                        {
                            right_border = j;
                            Right_Lost_Flag[i] = 0; //右丢线数组，丢线置1，不丢线置0
                            break;
                        }
                        if( j >= COLSIMAGE - 2 )//没找到右边界，把屏幕最右赋值给右边界
                        {
                            right_border = COLSIMAGE-1;
                            Right_Lost_Flag[i] = 1; //右丢线数组，丢线置1，不丢线置0
                            break;
                        }
                    }
                    for ( j = my_Mid; j > 0 + 1; j -- )//往左边扫描
                    {
                        if (index[i][j] == 2 )//黑黑白认为到达左边界
                        {
                            left_border = j;
                            Left_Lost_Flag[i] = 0; //左丢线数组，丢线置1，不丢线置0
                            break;
                        }
                        if( j <= 0 + 2 )
                        {
                            left_border = 0;//找到头都没找到边，就把屏幕最左右当做边界
                            Left_Lost_Flag[i] = 1; //左丢线数组，丢线置1，不丢线置0
                            break;
                        }
                    }
                    l_border[i].y = left_border;       //左边线线数组
                    r_border[i].y = right_border;      //右边线线数组
                }
                else//其余扫线
                {
                    for ( j = ( l_border[ i + 1 ].y + r_border[ i + 1 ].y ) / 2 ; j <= COLSIMAGE - 1; j ++ )
                    {
                        if ( index[i][j] == 2 )//找到2 为index边界
                        {
                            right_border = j;
                            Right_Lost_Flag[i] = 0; //右丢线数组，丢线置1，不丢线置0
                            break;
                        }
                        if( j >= COLSIMAGE - 2 )//没找到右边界，把屏幕最右赋值给右边界
                        {
                            right_border = COLSIMAGE-1;
                            Right_Lost_Flag[i] = 1; //右丢线数组，丢线置1，不丢线置0
                            break;
                        }
                    }
                    for (j = ( l_border[ i + 1 ].y + r_border[ i + 1 ].y ) / 2; j >= 0 + 1 ;j -- )//往左边扫描
                    {
                        if (index[i][j] == 2 )//找到2 为index边界
                        {
                            left_border = j;
                            Left_Lost_Flag[i] = 0; //左丢线数组，丢线置1，不丢线置0
                            break;
                        }
                        if( j <= 0 + 2 )
                        {
                            left_border = 0;//找到头都没找到边，就把屏幕最左右当做边界
                            Left_Lost_Flag[i] = 1; //左丢线数组，丢线置1，不丢线置0
                            break;
                        }
                    }
                    l_border[i].y = left_border;       //左边线线数组
                    r_border[i].y = right_border;      //右边线线数组
                }
            }
    }
    //basemap
    void Find_Mid_Line()
    {
        bool left=true;
        bool right=true;
        //从两边向中间巡线
        for(int i=0;i<ROWSIMAGE;i++)
        {
            left=true;
            right=true;
            l_border[i].y=0;
            r_border[i].y=COLSIMAGE-1;
            if(index[i][0]==255)
            {
                left=false;
            }
            if(index[i][COLSIMAGE-1]==255)
            {
                right=false;
            }
            if(left)
            {
                for(int j=0;j<COLSIMAGE;j++)
                {
                    if(index[i][j]==2)
                    {
                        l_border[i].y=j;
                        break;
                    } 
                }
            }
            if(right)
            {
                for(int j=COLSIMAGE-1;j>=0;j--)
                {
                    if(index[i][j]==2)
                    {
                        r_border[i].y=j;
                        break;
                    }
                }
            }
            if(right||left){
                effective_row[i]=true;
            }else{
                effective_row[i]=false;
            }
            
        }
    }

};
